/**
 * @file	mouse.c
 * @author	Mitch, David
 * @brief
 */

#include "header/common.h"
#include "header/os.h"
#include "roomba/roomba.h"
#include "roomba/roomba_sci.h"
#include "uart/uart.h"

#ifdef MOUSE

volatile uint8_t rxflag = 0;
/* event variable that is signaled or waited upon escaping mode */
EVENT *escapingEvent;

enum { A=1, B};
//const unsigned char PPP[] = {A,3};
//const unsigned int PT = sizeof(PPP)/2;
const unsigned char PPP[] = {};
const unsigned int PT = sizeof(PPP)/2;

unsigned int random = 0;

typedef enum _ir_dir
{
	IR_NORTH,
	IR_EAST,
	IR_SOUTH,
	IR_WEST,
	IR_NONE,
} IR_Direction_t;

IR_Direction_t direction = IR_NONE;

#define     clock8MHz()    CLKPR = _BV(CLKPCE); CLKPR = 0x00;

/** Get the North sensor reading.
 * @return 1 if sensor detects an IR signal, 0 if it does not.
 */
uint8_t get_IR_North()
{
	// if the N pin is low, then sensor detects an IR signal
	return (PINA & _BV(PORTA7)) == 0;
}

/** Get the East sensor reading.
 * @return 1 if sensor detects an IR signal, 0 if it does not.
 */
uint8_t get_IR_East()
{
	// if the E pin is low, then sensor detects an IR signal
	return (PINA & _BV(PORTA5)) == 0;
}

/** Get the South sensor reading.
 * @return 1 if sensor detects an IR signal, 0 if it does not.
 */
uint8_t get_IR_South()
{
	// if the S pin is low, then sensor detects an IR signal
	return (PINA & _BV(PORTA3)) == 0;
}

/** Get the South sensor reading.
 * @return 1 if sensor detects an IR signal, 0 if it does not.
 */
uint8_t get_IR_West()
{
	// if the W pin is low, then sensor detects an IR signal
	return (PINA & _BV(PORTA1)) == 0;
}

IR_Direction_t get_current_direction()
{
	// according to the Pololu documentation, only one line should
	// be high at a time.
	uint8_t input_reg = PINA;
	if ((input_reg & _BV(PORTA1)) == 0) return IR_WEST;
	if ((input_reg & _BV(PORTA3)) == 0) return IR_SOUTH;
	if ((input_reg & _BV(PORTA5)) == 0) return IR_EAST;
	if ((input_reg & _BV(PORTA7)) == 0) return IR_NORTH;

	return IR_NONE;
}

void roomba_Drive( int16_t velocity, int16_t radius )
{
	uart_putchar(DRIVE);
	uart_putchar(HIGH_BYTE(velocity));
	uart_putchar(LOW_BYTE(velocity));
	uart_putchar(HIGH_BYTE(radius));
	uart_putchar(LOW_BYTE(radius));
}

void delay_n_ticks() {
	/*
	unsigned int start = Now();
	unsigned int diff = Now() - start;
	while (diff < 100) {
		diff = Now() - start;
	}
	*/
	unsigned int tick = 0;
	unsigned int another = 0;
	while (tick < 1000) {
		tick++;
		another = 0;
		while (another < 500) {
			another++;
		}
	}
}

void search_task(void)
{
    for(;;)
    {
//		switch (random) {
		switch (Now() % 4) {
			case 0  :
				roomba_Drive(100, 0x0001);
				break;
			case 1  :
				roomba_Drive(100, 0xFFFF);
				break;
			case 2  :
				roomba_Drive(0xFF9C, 0x8000);
				break;
			case 3  :
				roomba_Drive(100, 0x8000);
				break;
			default :
				roomba_Drive(0, 0);
				break;
		}
		delay_n_ticks();
		Task_Next();
		/*
		random++; if (random >= 4) random = 0;
		 */
    }
}

void sensor_task(void)
{
    for(;;)
    {
    	unsigned int i;
    	i = 0;
    	while (i < 1000) {
			direction = get_current_direction();
			if (direction == IR_WEST) {
				roomba_Drive(50, 0x0001);
				//roomba_Drive(100, 0x0001);
				Event_Signal(escapingEvent);
			}
			else if (direction == IR_EAST) {
				roomba_Drive(50, 0xFFFF);
				//roomba_Drive(100, 0xFFFF);
				Event_Signal(escapingEvent);
			}
			else if (direction == IR_SOUTH) {
				roomba_Drive(0xFFCE, 0x8000);
				//roomba_Drive(0xFF9C, 0x8000);
				Event_Signal(escapingEvent);
			}
			else if (direction == IR_NORTH) {
				roomba_Drive(50, 0x8000);
				//roomba_Drive(100, 0x8000);
				Event_Signal(escapingEvent);
			}
			else {
				//turn off all leds
				//PORTD &= 0x00;
				//stop roomba
				//roomba_Drive(0, 0);
			}

			i++;
    	}
    	Task_Next();
    }
}

/**
 * @brief Send command to Roomba
 */
void task_escaping(void) {
	unsigned int detected = 0;
	for (;;) {
		Event_Wait(escapingEvent);
		detected = 1;
		while (detected == 1) {
			if (direction == IR_WEST) {
				//roomba_Drive(50, 0xFFFF);
				//roomba_Drive(100, 0xFFFF);
				roomba_Drive(100, 0xFFFF);
				//roomba_Drive(250, 0xFFFF);
				delay_n_ticks();
			}
			else if (direction == IR_EAST) {
				//roomba_Drive(50, 0x0001);
				//roomba_Drive(100, 0x0001);
				roomba_Drive(100, 0x0001);
				//roomba_Drive(250, 0x0001);
				delay_n_ticks();
			}
			else if (direction == IR_SOUTH) {
				//roomba_Drive(50, 0x8000);
				//roomba_Drive(100, 0x8000);
				roomba_Drive(100, 0x8000);
				//roomba_Drive(250, 0x8000);
				delay_n_ticks();
			}
			else if (direction == IR_NORTH) {
				//roomba_Drive(0xFFCE, 0x8000);
				roomba_Drive(0xFF9C, 0x8000);
				//roomba_Drive(0xFF08, 0x8000);
				//roomba_Drive(0xFE96, 0x8000);
				delay_n_ticks();
			} else {
				detected = 0;
			}

			direction = get_current_direction();
		}
	}
}

int main(void) {

	clock8MHz();

	//initialize Beacon
	// set IR pins to input (DDR = 0bx0x0x0x0), PORTA7 = NORTH, PORTA5 = EAST, PORTA3 = SOUTH, PORTA1 = WEST
	DDRA &= ~(_BV(PORTA7) | _BV(PORTA5) | _BV(PORTA3) | _BV(PORTA1));
	PORTA |= (_BV(PORTA7) | _BV(PORTA5) | _BV(PORTA3) | _BV(PORTA1));

	//initialize leds
	DDRD = (1<<PORTD4) | (1<<PORTD5) | (1<<PORTD6) | (1<<PORTD7);

	Roomba_Init();
	escapingEvent = Event_Init();
    Task_Create(task_escaping, 1, SYSTEM, 1);
    //Task_Create(generic_task, 0, SYSTEM, 1);

	Task_Create(sensor_task, 0, BRR, 0);
	Task_Create(search_task, 0, BRR, 0);

    return 0;
}
void radio_rxhandler(uint8_t pipenumber)
{
	rxflag = 1;
	//PORTD ^= _BV(PD7);
}
#endif
